import os, sqlite3, time
from socket import socket, AF_INET, SOCK_DGRAM
from time import sleep
from settings import Settings
from struct import pack,unpack
from datetime import datetime, timedelta

class VSAT:
    def __init__(self,prn,elev=None,azi=None,snr=None):
        if type(prn) is str:
            try:
                self.prn, self.elev, self.azi, self.snr = map(ord,prn)
            except:
                print "VSAT COULD NOT PARSE RIGHT"
        else:    
            self.prn = prn
            self.elev = elev
            self.azi = azi
            self.snr = snr
        #print "PRN = ",self.prn,"\tELEV = ",self.elev,"\tAZI = ",self.azi,"\tSNR = ",self.snr
        
    def __eq__(self,other):
        if other is None:return False
        return self.prn == other.prn and self.elev == other.elev and self.azi == other.azi and self.snr == other.snr
    
    def __ne__(self,other):
        return not self == other
        
    def toXML(self):
        xml  = "\t\t<vsat>\n"
        xml += "\t\t\t<prn>"+str(self.prn)+"</prn>\n"
        xml += "\t\t\t<elev>"+str(self.elev)+"</elev>\n"
        xml += "\t\t\t<azi>"+str(self.azi)+"</azi>\n"
        xml += "\t\t\t<snr>"+str(self.snr)+"</snr>\n"
        xml += "\t\t</vsat>\n"
        return xml 

class PIDconfig:
    def __init__(self,data):
        self.p, self.i, self.d, self.max, self.min = unpack("f"*5,data[:20])
        self.settingsFrom(ord(data[20]))
    
    def __eq__(self,other):
        if other is None:return False
        return self.p == other.p and self.i == other.i and self.d == other.d and self.max == other.max and self.settingsTo() == other.settingsTo()
    
    def __ne__(self,other):
        return not self == other
    
    def toData(self):
        return pack("f"*5,self.p,self.i,self.d,self.max,self.min)+self.settingsTo()
    
    def settingsFrom(self,settings):
        self.maxEn = (settings&1) != 0
        self.minEn = (settings&2) != 0
        self.invert = (settings&4) != 0
        self.enable = (settings&8) != 0
    
    def settingsTo(self):
        return chr((self.enable<<3)|(self.invert<<2)|(self.minEn<<1)|self.maxEn)
    
    def toXML(self,name):
        xml  = "\t<"+name+">\n"
        xml += "\t\t<p>"+str(self.p)+"</p>\n"
        xml += "\t\t<i>"+str(self.i)+"</i>\n"
        xml += "\t\t<d>"+str(self.d)+"</d>\n"
        xml += "\t\t<max>"+str(self.max)+"</max>\n"
        xml += "\t\t<min>"+str(self.min)+"</min>\n"
        xml += "\t\t<invert>"+str(self.invert)+"</invert>\n"
        xml += "\t\t<maxEn>"+str(self.maxEn)+"</maxEn>\n"
        xml += "\t\t<minEn>"+str(self.minEn)+"</minEn>\n"
        xml += "\t</"+name+">\n"
        return xml
        
class AT:
    s = None #socket
    data = "" #the packet data
    inPacket = False
    packetId = 0
    packetSz = 0
    pacetChecksum = 0
    udpTo = None
    thread_go = True
    newPacket = {}
    ack = {}
    connectionProblem = False
    ackList = [1,10,11,17]
    
    c = None #SQLite connection
    fl = None #SQLite flight number
    
    #=====airplane info=====#
    #System status information
    apMode = False
    cells = [None for x in range(4)]
    
    #telemetry
    lat = lon = alt = roll = pitch = heading = speed = None
    
    #servo stuff
    svo_ail = svo_elev = svo_th = svo_rud = None
    
    #gps stuff
    gpsStatus = gpsLat = gpsLon = gpsAlt = gpsSpeed = gpsHDOP = gpsVDOP = gpsPDOP = gpsCourse = gpsDeviation = None
    gpsVsat = []
    
    pidNames = ["roll","heading","pitch","vrate","altitude","throttle","slip"]
    pids = [None for x in pidNames]
    
    #plane modes
    planeMode = planeModeIndv = 0
    
    #setpoints
    sp_roll = sp_heading = sp_pitch = sp_vrate = sp_altitude = sp_throttle = sp_slip = None
    
    #sensors
    gx = gy = gz = ax = ay = az = cx = cy = cz = baro = None
    
    ##### X M L   G E T T E R S ############################################################### X M L   G E T T E R S #####
    
    @staticmethod
    def getTelemXML():
        now = datetime.now()
        if 7 not in AT.newPacket or (now-AT.newPacket[7]).seconds > 1:
            AT.out_telemReq()
            AT.lat = AT.lon = AT.alt = AT.roll = AT.pitch = AT.heading = AT.speed = None
        xml  = "<telem>\n"
        xml += "\t<lat>"+str(AT.lat)+"</lat>\n"
        xml += "\t<lon>"+str(AT.lon)+"</lon>\n"
        xml += "\t<alt>"+str(AT.alt)+"</alt>\n"
        xml += "\t<roll>"+str(AT.roll)+"</roll>\n"
        xml += "\t<pitch>"+str(AT.pitch)+"</pitch>\n"
        xml += "\t<heading>"+str(AT.heading)+"</heading>\n"
        xml += "\t<speed>"+str(AT.speed)+"</speed>\n"
        xml += "\t<ap>"+str(AT.apMode)+"</ap>\n"
        xml += "</telem>\n"
        return xml
    
    @staticmethod
    def getGPSxml():
        now = datetime.now()
        if 15 not in AT.newPacket or (now-AT.newPacket[7]).seconds > 1 or AT.gpsStatus is 0:
            AT.gpsStatus = 0
            AT.gpsLat = AT.gpsLon = AT.gpsAlt = AT.gpsSpeed = AT.gpsHDOP = AT.gpsVDOP = AT.gpsPDOP = AT.gpsCourse = AT.gpsDeviation = None
            AT.gpsVsat = []
        xml  = "<gps>\n"
        xml += "\t<status>"+str(AT.gpsStatus)+"</status>\n"
        xml += "\t<lat>"+str(AT.gpsLat)+"</lat>\n"
        xml += "\t<lon>"+str(AT.gpsLon)+"</lon>\n"
        xml += "\t<alt>"+str(AT.gpsAlt)+"</alt>\n"
        xml += "\t<speed>"+str(AT.gpsSpeed)+"</speed>\n"
        xml += "\t<hdop>"+str(AT.gpsHDOP)+"</hdop>\n"
        xml += "\t<vdop>"+str(AT.gpsVDOP)+"</vdop>\n"
        xml += "\t<pdop>"+str(AT.gpsPDOP)+"</pdop>\n"
        xml += "\t<course>"+str(AT.gpsCourse)+"</course>\n"
        xml += "\t<deviation>"+str(AT.gpsDeviation)+"</deviation>\n"
        xml += "\t<activeSats>"+str(len([x for x in AT.gpsVsat if x.snr > 0]))+"</activeSats>\n"
        xml += "\t<vsats>\n"
        for vsat in AT.gpsVsat:
            xml += vsat.toXML()
        xml += "\t</vsats>\n"
        xml += "</gps>\n"
        return xml
    
    @staticmethod
    def getPIDxml():
        if AT.refreshPackets(13) is False:
            return "<pid>\n\t<error>True</error>\n</pid>\n"
        xml  = "<pid>\n"
        xml += "\t<error>False</error>\n"
        for pos, pid in enumerate(AT.pids):
            xml += pid.toXML(AT.pidNames[pos])
        xml += "</pid>\n"
        return xml
    
    @staticmethod
    def changePID(name,p,i,d,max,min,inv,maxEn,minEn):
        name = name.replace("pid_","")
        if AT.pids[0] is None:return # no connection yet
        idx = AT.pidNames.index(name)
        try:
            thepid = AT.pids[idx]
        except:
            err = "NAME NOT FOUND FOR PIDS: "+name
            print err
            return err
        thepid.p = float(p)
        thepid.i = float(i)
        thepid.d = float(d)
        thepid.max = float(max)
        thepid.min = float(min)
        thepid.minEn = minEn.lower()=="true"
        thepid.maxEn = maxEn.lower()=="true"
        thepid.invert = inv.lower()=="true"
        AT.out_pid_config(thepid)
        
    @staticmethod
    def getAPxml():
        if AT.refreshPacketsG([2,16]) is False:return "<ap>\n\t<error>True</error>\n</ap>\n"
        xml  = "<ap>\n"
        xml += "\t<error>False</error>\n"
        xml += "\t<mode>"+str(AT.planeMode)+"</mode>\n"
        xml += "\t<indv>\n";
        xml += "\t\t<roll>"+str(AT.planeModeIndv&3)+"</roll>\n"
        xml += "\t\t<pitch>"+str((AT.planeModeIndv&12)>>2)+"</pitch>\n"
        xml += "\t\t<throttle>"+str((AT.planeModeIndv&16)!=0)+"</throttle>\n"
        xml += "\t\t<slip>"+str((AT.planeModeIndv&32)!=0)+"</slip>\n"
        xml += "\t</indv>\n"
        xml += "\t<setpoints>\n"
        xml += "\t\t<roll>"+str(AT.sp_roll)+"</roll>\n"
        xml += "\t\t<heading>"+str(AT.sp_heading)+"</heading>\n"
        xml += "\t\t<pitch>"+str(AT.sp_pitch)+"</pitch>\n"
        xml += "\t\t<vrate>"+str(AT.sp_vrate)+"</vrate>\n"
        xml += "\t\t<alt>"+str(AT.sp_altitude)+"</alt>\n"
        xml += "\t\t<throttle>"+str(AT.sp_throttle)+"</throttle>\n"
        xml += "\t\t<slip>"+str(AT.sp_slip)+"</slip>\n"
        xml += "\t</setpoints>\n"
        xml += "</ap>\n"
        return xml
    
    @staticmethod
    def changeSetpoints(roll,heading,pitch,vrate,altitude,throttle,slip):
        roll,heading,pitch,vrate,altitude,throttle,slip = map(float,[roll,heading,pitch,vrate,altitude,throttle,slip])
        if AT.sp_roll != roll:AT.out_setpointAdjust(1, roll)
        if AT.sp_heading != heading:AT.out_setpointAdjust(2, heading)
        if AT.sp_pitch != pitch:AT.out_setpointAdjust(3, pitch)
        if AT.sp_vrate != vrate:AT.out_setpointAdjust(4,vrate)
        if AT.sp_altitude != altitude:AT.out_setpointAdjust(5,altitude)
        if AT.sp_throttle != throttle:AT.out_setpointAdjust(6,throttle)
        if AT.sp_slip != slip:AT.out_setpointAdjust(7,slip)
        
    @staticmethod
    def changeMode(mode):
        mode = int(mode)
        if mode>3:
            print "MODE IS > 3!!!!"
            return
        AT.planeMode = mode
        AT.out_planeMode()
    
    @staticmethod
    def changeIndvMode(mode,val):
        val = int(val)
        if "roll" in mode:
            AT.planeModeIndv &= ~3&0xFF
        elif "pitch" in mode:
            AT.planeModeIndv &= ~12&0xFF
            val<<=2
        elif "throttle" in mode:
            AT.planeModeIndv &= ~16&0xFF
            val<<=4
        elif "slip" in mode:
            AT.planeModeIndv &= ~32&0xFF
            val<<=5
        AT.planeModeIndv |= val
        AT.out_planeMode()
        
    @staticmethod
    def getSensorsXML():
        gotit = AT.arePacketsRecent(3)
        xml  = "<sensors>\n"
        xml += "\t<error>"+str(not gotit)+"</error>\n"
        xml += "\t<gx>"+str(AT.gx)+"</gx>\n"
        xml += "\t<gy>"+str(AT.gy)+"</gy>\n"
        xml += "\t<gz>"+str(AT.gz)+"</gz>\n"
        xml += "\t<ax>"+str(AT.ax)+"</ax>\n"
        xml += "\t<ay>"+str(AT.ay)+"</ay>\n"
        xml += "\t<az>"+str(AT.az)+"</az>\n"
        xml += "</sensors>\n"
        return xml
    
    @staticmethod
    def arePacketsRecent(packets):
        if type(packets) is int:packets = [packets]
        t = datetime.now()-timedelta(seconds=1)
        for num in packets:
            if not AT.goodResp(num, t):
                return False
        return True
        
    @staticmethod
    def verifyPackets(packets):
        if type(packets) is int:packets = [packets]
        t = datetime.now() #record the starting time
        badPackets = []
        for num in packets:
            if AT.goodResp(num, t):
                AT.out(num,False)
                badPackets.append(num)
        while len(badPackets)>0:
            if AT.dt(t) > 2:return False#timeout
            for num in badPackets:
                if AT.goodResp(num, t):
                    badPackets.remove(num)#its all good
            sleep(0.01)#don't kill the processor plz
        return True
    
    @staticmethod
    def refreshPackets(packets):
        if type(packets) is int:packets = [packets]
        t = datetime.now() #record the starting time
        for num in packets:AT.out(num,False)
        while len(packets)>0:
            if AT.dt(t) > 2:return False#timeout
            for num in packets:
                if AT.goodResp(num, t):
                    packets.remove(num)#its all good
            sleep(0.01)#don't kill the processor plz
        return True
    
    @staticmethod
    def refreshPacketsG(packets,interval=0.25):
        #guarantee request
        if type(packets) is int:packets = [packets]
        t = datetime.now() #record the starting time
        reqAt = 0 # request intervals
        while len(packets)>0:
            dt = AT.dt(t)
            if dt > 2:return False#timeout
            if dt > reqAt*interval:
                for num in packets:AT.out(num,False)#do not guarentee this packet, this function will do that
                reqAt+=1
            else:
                for num in packets:
                    if AT.goodResp(num, t):
                        packets.remove(num)#its all good
            sleep(0.01)#don't kill the processor plz
        return True
    
    @staticmethod
    def getAllFlights():
        try:
            xml  = "<flights>\n"
            if AT.c is None:
                xml += "\t<error>true</error>\n"
                xml += "\t<reason>DB not connected</reason>\n"
            else:
                xml += "\t<error>false</error>\n"
                conn, c = AT.startSQLdb()
                c.execute('SELECT dt, flight FROM flight')
                for flight in c.fetchall():
                   # print "FLIGHT ------------------- ",flight
                    fltime = flight[0]
                    number = flight[1]
                    c.execute("SELECT max(dt), min(dt) FROM telem where flight = ?",(number,))
                    end, start = c.fetchone()
                    #print start, end
                    if end is None:
                        durration = 0
                    else:
                        durration = end - start
                    xml += "\t<flight>\n"
                    xml += "\t\t<number>"+str(number)+"</number>\n"
                    if start is None:
                        xml += "\t\t<time>N/A</time>\n"
                    else:
                        xml += "\t\t<time>"+str(time.ctime(fltime))+"</time>\n"
                    xml += "\t\t<durration>"+str(timedelta(seconds=durration))+"</durration>\n"
                    xml += "\t</flight>\n"
                c.close()
            xml += "</flights>\n"
            return xml
        except Exception, e:
            return "<flights>\n\t<error>true</error>\n\t<reason>"+str(e)+"</reason>\n</flights>\n"
            raise e
    
    @staticmethod
    def delFlight(num):
        xml  = "<status>\n"
        conn, c = AT.startSQLdb()
        c.execute("SELECT count(*) FROM flight WHERE flight = ?;",(num,))
        if int(c.fetchone()[0]) == 0:
            xml += "\t<error>true</error>\n"
            xml += "\t<reason>Flight number does not exist</reason>\n"
            print "Flight not found!"
        else:
            try:
                c.execute("DELETE FROM flight WHERE flight = ?;",(num,))
                c.execute("DELETE FROM telem WHERE flight = ?;",(num,))
                c.execute("DELETE FROM gps WHERE flight = ?;",(num,))
                c.execute("DELETE FROM pidConfig WHERE flight = ?;",(num,))
                c.execute("DELETE FROM sensors WHERE flight = ?;",(num,))
                c.execute("DELETE FROM batteryLog WHERE flight = ?;",(num,))
                conn.commit()
                xml += "\t<error>false</error>\n"
            except e:
                xml += "\t<error>true</error>\n"
                xml += "\t<reason>"+str(e)+"</reason>\n"
        c.close()
        
        xml += "</status>\n"
        return xml
    
    @staticmethod
    def dt(a,b=None):
        if b is None:c = datetime.now()-a
        else:c = b-a
        return c.seconds+(c.microseconds/1000000.0)
    
    @staticmethod
    def goodResp(id,t):
        if id in AT.ackList:
             return (id in AT.ack and AT.ack[id]>t)
        return (id in AT.newPacket and AT.newPacket[id]>t)
    
    ##### G O O G L E   E A R T H   E X P O R T ############################### G O O G L E   E A R T H   E X P O R T #####
    
    @staticmethod
    def googleEarthKMLexport(num):
        conn, c = AT.startSQLdb()
        c.execute("SELECT dt FROM flight WHERE flight = ?",(num,))
        dt = c.fetchone()[0]
        
        fn = str(dt)+"_FL"+str(num)
        
        kml  = "<?xml version=\"1.0\" encoding=\"UTF-8\"?>\n"
        kml += "<kml xmlns=\"http://www.opengis.net/kml/2.2\" xmlns:gx=\"http://www.google.com/kml/ext/2.2\" xmlns:kml=\"http://www.opengis.net/kml/2.2\" xmlns:atom=\"http://www.w3.org/2005/Atom\">\n"
        kml += "<Document>\n"
        kml += "\t<name>"+fn+"</name>\n"
        kml += "\t<Folder>\n"
        kml += "\t"*2+"<name>Telemetry</name>\n"
        kml += "\t"*2+"<open>1</open>\n"
        
        apNodes = []
        manualNodes = []
        
        on = None
        c.execute("SELECT * from telem WHERE flight = ?",(num,))
        for row in c.fetchall():
            if row[2] == 0 and row[3] == 0 and row[4] == 0:continue
            apon = row[-1]
            if apon != on:
                if apon:apNodes.append([])
                else:manualNodes.append([])
                on = apon
            if apon:myNode = apNodes[-1]
            else:myNode = manualNodes[-1]
            myNode.append(row)
        
        kml += "\t"*2+"<Folder>\n"
        kml += "\t"*3+"<name>Autopilot</name>\n"
        kml += "\t"*3+"<open>0</open>\n"
        
        for x in apNodes:
            kml += "\t"*3+"<Placemark>\n"
            kml += "\t"*4+"<name>Autopilot from "+str(time.ctime(x[0][0]))+" to "+str(time.ctime(x[-1][0]))+" --- "+str(timedelta(seconds=x[-1][0]-x[0][0]))+"</name>\n"
            kml += "\t"*4+"<styleUrl>#m_ylw-pushpin0</styleUrl>\n"
            kml += "\t"*4+"<LineString>\n"
            kml += "\t"*5+"<tessellate>1</tessellate>\n"
            kml += "\t"*5+"<altitudeMode>absolute</altitudeMode>\n"
            kml += "\t"*5+"<coordinates>\n"+"\t"*6
            for y in x:kml += ",".join(map(str,(y[3],y[2],y[4]*0.3048)))+" "
            kml += "\n"+"\t"*5+"</coordinates>\n"
            kml += "\t"*4+"</LineString>\n"
            kml += "\t"*3+"</Placemark>\n"
        
        kml += "\t"*2+"</Folder>\n"
        kml += "\t"*2+"<Folder>\n"
        kml += "\t"*3+"<name>Manual</name>\n"
        kml += "\t"*3+"<open>0</open>\n"
        
        for x in manualNodes:
            kml += "\t"*3+"<Placemark>\n"
            kml += "\t"*4+"<name>Manual from "+str(time.ctime(x[0][0]))+" to "+str(time.ctime(x[-1][0]))+" --- "+str(timedelta(seconds=x[-1][0]-x[0][0]))+"</name>\n"
            kml += "\t"*4+"<styleUrl>#m_ylw-pushpin0</styleUrl>\n"
            kml += "\t"*4+"<LineString>\n"
            kml += "\t"*5+"<tessellate>1</tessellate>\n"
            kml += "\t"*5+"<altitudeMode>absolute</altitudeMode>\n"
            kml += "\t"*5+"<coordinates>\n"+"\t"*6
            for y in x:kml += ",".join(map(str,(y[3],y[2],y[4]*0.3048)))+" "
            kml += "\n"+"\t"*5+"</coordinates>\n"
            kml += "\t"*4+"</LineString>\n"
            kml += "\t"*3+"</Placemark>\n"
        
        kml += "\t"*2+"</Folder>\n"
        kml += "\t</Folder>\n"
        
        #TODO: need to make the voltage stuff
        
        kml += "</Document>\n"
        kml += "</kml>\n"
        
        c.close()
        
        try:
            f = file("/Users/jake/Desktop/"+fn+".kml","w")
            f.write(kml)
            f.close()
            print "Done exporting ",fn
        except Exception, e:
            print e
            
    ##### C S V    E X P O R T ################################################################# C S V    E X P O R T #####
    
    @staticmethod
    def CSVexport(num):
        try:
            data = ["dt","ax","ay","az","gx","gy","gz"]
            cdata = ",".join(data)
            conn, c = AT.startSQLdb()
            c.execute("SELECT "+cdata+" FROM sensors WHERE flight = ?",(num,))
            
            csv = cdata+"\n"
            for row in c.fetchall():csv += ",".join(map(str,row))+"\n"
                
            c.execute("SELECT dt FROM flight WHERE flight = ?",(num,))
            dt = c.fetchone()[0]
            
            fn = str(dt)+"_FL"+str(num)+"_sensors"
            
            try:
                f = file("/Users/jake/Desktop/"+fn+".csv","w")
                f.write(csv)
                f.close()
                print "Done exporting ",fn
            except Exeption, e:
                print e
            
            data = ["dt","lat","lon","alt","speed","course"]
            cdata = ",".join(data)
            conn, c = AT.startSQLdb()
            c.execute("SELECT "+cdata+" FROM gps WHERE flight = ?",(num,))
            
            csv = cdata+"\n"
            for row in c.fetchall():csv += ",".join(map(str,row))+"\n"
            
            fn = str(dt)+"_FL"+str(num)+"_gps"
            
            c.close()
            
            try:
                f = file("/Users/jake/Desktop/"+fn+".csv","w")
                f.write(csv)
                f.close()
                print "Done exporting ",fn
            except Exception, e:
                print e
        except Exception, e:
            print "Could not export: ",e
    
    ##### M A I N   P A C K E T   C O N T R O L ############################### M A I N   P A C K E T   C O N T R O L #####
    
    @staticmethod
    def packet(input):
        
        if AT.c is not None and AT.fl is None:
            AT.sqlStartFlight()
        
        AT.data += input
        while True:
            if AT.inPacket is True: #in packet
                if len(AT.data) < AT.packetSz:return # full stop, do not have the rest of the packet, wait for more
                AT.inPacket = False
                checksum = AT.packetId ^ AT.packetSz #this is the first part of the checksum checks
                for byte in map(ord,AT.data[:AT.packetSz]):checksum ^= byte# check the rest of the bytes in the pacet
                if checksum != AT.packetChecksum:continue #the checksum needs to match up, or just skip this packet
                
                if   AT.packetId is  0:AT.in_ack()
                elif AT.packetId is  1:AT.in_err()
                elif AT.packetId is  2:AT.in_planeModeResp()
                elif AT.packetId is  3:AT.in_sensorReadings()
                elif AT.packetId is  7:AT.in_telem()
                elif AT.packetId is  8:AT.in_systemStatus()
                elif AT.packetId is 12:AT.in_servoConfig()
                elif AT.packetId is 13:AT.in_pidConfig()
                elif AT.packetId is 14:AT.in_servos()
                elif AT.packetId is 15:AT.in_gps()
                elif AT.packetId is 16:AT.in_setpoints()
                else:AT.in_unknown()
                
                AT.newPacket[AT.packetId] = datetime.now()
                AT.data = AT.data[AT.packetSz:]# remove the packet, it has been read
            else: #not in packet
                if len(AT.data)<5:return#if we do not have enough packets to be a complete header. Full stop on parsing, wait for more data
                if AT.data[:2] != "$<":AT.data = AT.data[1:]#remove the first charecter, its corrupted
                else:
                    AT.packetChecksum, AT.packetId, AT.packetSz = map(ord,AT.data[2:5]) #pull out the header info
                    AT.data = AT.data[5:]#remove the header, we got it's data
                    AT.inPacket = True #now in packet

    ##### I N P U T S ########################################################################## I N P U T S #####

    @staticmethod
    def in_ack():
        AT.ack[ord(AT.data[0])] = datetime.now()
    
    @staticmethod
    def in_err():
        tt = "Error packet: "+str(ord(AT.data[0]))
        if AT.packetSz >= 1:
            tt += "\tError: "
            errNum = ord(AT.data[1])
            if errNum is 0:tt += "General error"
            elif errNum is 1:tt += "Body Size"
            elif errNum is 2:tt += "Invalid Byte -> "+str(ord(AT.data[2]))
            else:tt += str(errNum)
        try:
            f = open(os.getenv("HOME")+"/Documents/AutoTruckerError.log");
            f.write(tt)
            f.close()
        except:print"Could not write the error log to a file"
        print tt
    
    @staticmethod
    def in_planeModeResp():
        AT.planeMode = ord(AT.data[0])
        AT.planeModeIndv = ord(AT.data[1])
    
    @staticmethod
    def in_sensorReadings():
        AT.gx, AT.gy, AT.gz, AT.ax, AT.ay, AT.az, AT.cx, AT.cy, AT.cz, AT.baro = unpack("f"*10,AT.data[:40])
        AT.sqlInSensors()
        #print "SENSORS "*20
    
    @staticmethod
    def in_telem():
        AT.lat, AT.lon, AT.alt, AT.roll, AT.pitch, AT.heading, AT.speed = unpack("f"*7,AT.data[:28])
        AT.sqlInTelem()
    
    @staticmethod
    def in_systemStatus():
        #print "SYSTEM STATUS "*10
        #print AT.apMode, AT.cells
        AT.apMode = (ord(AT.data[0])==1)
        AT.cells = unpack("H"*4,AT.data[1:9])
    
    @staticmethod
    def in_servoConfig():
        pass
    
    @staticmethod
    def in_pidConfig():
        oldpids = AT.pids
        AT.pids = [PIDconfig(AT.data[x*21:(x+1)*21]) for x in xrange(len(AT.pids))]
        for x,pid in enumerate(AT.pids):
            if pid != oldpids[x]:AT.sqlChangePID(x,pid)
    
    @staticmethod
    def in_servos():
        AT.svo_ail, AT.svo_elev, AT.svo_th, AT.svo_rud = unpack("H"*4,AT.data)
    
    @staticmethod
    def in_gps():
        AT.gpsStatus = ord(AT.data[0])
        AT.gpsLat, AT.gpsLon, AT.gpsAlt, AT.gpsSpeed, AT.gpsHDOP, AT.gpsVDOP, AT.gpsPDOP, AT.gpsCourse, AT.gpsDeviation = unpack("f"*9,AT.data[1:37])
        count = ord(AT.data[37])
        AT.gpsVsat = []
        for x in xrange(count):
            AT.gpsVsat.append(VSAT(AT.data[38+(x*4):38+((x+1)*4)]))
        AT.sqlInGPS()
    
    @staticmethod
    def in_setpoints():
        AT.sp_roll, AT.sp_heading, AT.sp_pitch, AT.sp_vrate, AT.sp_altitude, AT.sp_throttle, AT.sp_slip = unpack("f"*7,AT.data[:28])
    
    @staticmethod
    def in_unknown():
        print "Unknown packet id: ",AT.packetId
    
    ##### O U T P U T S ######################################################################## O U T P U T S #####
    
    @staticmethod
    def out(id,body,g=None):
        if g is None:#support for the simple packets
            g = body
            body = ""
        if g:AT._out_packetG(id, body)
        else:AT._out_packet(id, body)
        
    @staticmethod
    def _out_packet(id,body):
        checksum = id^len(body)
        for byte in body:checksum ^= ord(byte)
        message = "@>"+chr(checksum)+chr(id)+chr(len(body))+body
        #print map(ord,message)
        try:
            AT.s.sendto(message,AT.udpTo)
            AT.connectionProblem = False
        except:
            AT.connectionProblem = True
            print "X"*30
    
    @staticmethod
    def _out_packetG(id,body,interval=0.25,timeout=2.0):
        #send guarenteed
        i=0
        t = datetime.now()
        while not AT.goodResp(id, t):
            dt = AT.dt(t)
            if dt > timeout:
                print "Timeout on sending packet"
                return
            AT.out_packet(id, body)
            sleep(interval)
    
    @staticmethod
    def out_planeMode(g=False):
        print str(AT.planeModeIndv)
        AT.out(1, chr(AT.planeMode)+chr(AT.planeModeIndv),g)
    
    @staticmethod
    def out_planeModeReq(g=False):
        AT.out(2,g)
        
    @staticmethod
    def out_sensorReadingsReq(g=False):
        AT.out(3,g)
    
    @staticmethod
    def out_telemReq(g=False):
        AT.out(7,False)
        
    @staticmethod
    def out_pid_config(pid = None,g=False):
        if pid is None:
            AT.out(11,chr(0x7F)+''.join([x.toData() for x in AT.pids]),g)
        else:
            AT.out(11,chr(1<<AT.pids.index(pid))+pid.toData(),g)
    
    @staticmethod
    def out_pidReq(g=False):
        AT.out(13,g)
    
    @staticmethod
    def out_servoPositionsReq(g=False):
        AT.out(14,g)
    
    @staticmethod
    def out_gpsReq(g=False):
        AT.out(15,g)
    
    @staticmethod
    def out_setpointReq(g=False):
        AT.out(16,g)
    
    @staticmethod
    def out_setpointAdjust(sp_num,val,g=False):
        AT.out(17, chr(sp_num)+pack("f",val),g)
        
    ##### S Q L I T E ########################################################################## S Q L I T E #####
    
    @staticmethod
    def startSQLdb():
        conn = sqlite3.connect(os.path.expanduser(Settings.getAttr("db/file")))
        return (conn,conn.cursor())
    
    @staticmethod
    def sqlStartFlight():
        t = (int(time.time()),)
        conn, c = AT.startSQLdb()
        c.execute("INSERT INTO flight (dt) VALUES (?);",t)
        c.execute("SELECT last_insert_rowid();")
        AT.fl = int(c.fetchone()[0])
        conn.commit()
        c.close()
        print "STARTED FLIGHT ",AT.fl," @ ",t[0]
    
    @staticmethod
    def sqlInTelem():
        if AT.c is None or AT.fl is None:return
        conn, c = AT.startSQLdb()
        #dt INT NOT NULL, flight INT NOT NULL, lat REAL, lon REAL, alt REAL, roll REAL, pitch REAL, heading REAL, speed REAL, autopiloton INT
        t = (int(time.time()),AT.fl,AT.lat,AT.lon,AT.alt,AT.roll,AT.pitch,AT.heading,AT.speed,AT.apMode)
        c.execute("INSERT INTO telem VALUES ("+",".join(["?" for x in t])+");",t)
        conn.commit()
        c.close()
    
    @staticmethod
    def sqlInGPS():
        if AT.c is None or AT.fl is None:return
        t = (int(time.time()), AT.fl, AT.gpsStatus, AT.gpsLat, AT.gpsLon, AT.gpsAlt, AT.gpsSpeed, AT.gpsHDOP, AT.gpsVDOP, AT.gpsPDOP, AT.gpsCourse, AT.gpsDeviation)
        conn, c = AT.startSQLdb()
        c.execute("INSERT INTO gps VALUES ("+",".join(["?" for x in t])+");",t)
        conn.commit()
        c.close()
    
    @staticmethod
    def sqlChangePID(num,pid):
        if AT.c is None or AT.fl is None:return
        #dt INT NOT NULL, flight INT NOT NULL, which INT, p REAL NOT NULL, i REAL NOT NULL, d REAL NOT NULL, max REAL NOT NULL, min REAL NOT NULL, settings INT NOT NULL
        t = (int(time.time()), AT.fl, num, pid.p, pid.i, pid.d, pid.max, pid.min, pid.settingsTo())
        conn, c = AT.startSQLdb()
        c.execute("INSERT INTO pidConfig VALUES ("+",".join(["?" for x in t])+");",t)
        conn.commit()
        c.close()
        
    @staticmethod
    def sqlInSensors():
        if AT.c is None or AT.fl is None:return
        #"ax","ay","az","gx","gy","gz","cx","cy","cz","baro"
        t = (int(time.time()), AT.fl, AT.ax, AT.ay, AT.az, AT.gx, AT.gy, AT.gz, AT.cx, AT.cy, AT.cz, AT.baro)
        conn, c = AT.startSQLdb()
        c.execute("INSERT INTO sensors VALUES ("+",".join("?" for x in t)+");",t)
        conn.commit()
        c.close()
    
    
    ##### T H R E A D ########################################################################## T H R E A D #####

    @staticmethod
    def thread():
        AT.s = socket( AF_INET, SOCK_DGRAM ) # UDP
        #s.setsockopt(socket.SOL_SOCKET, socket.SO_REUSEADDR, 1)
        AT.udpTo = (Settings.getAttr("gps/ip"),int(Settings.getAttr("gps/port")))
        #print "@#"*20,AT.udpTo
        
        try:
            #os.remove("data.db")
            conn, c = AT.startSQLdb()
            c.execute("CREATE TABLE IF NOT EXISTS flight (dt INT NOT NULL, flight INTEGER PRIMARY KEY);")
            c.execute("CREATE TABLE IF NOT EXISTS telem (dt INT NOT NULL, flight INT NOT NULL, lat REAL, lon REAL, alt REAL, roll REAL, pitch REAL, heading REAL, speed REAL, autopiloton INT);")
            c.execute("CREATE TABLE IF NOT EXISTS gps (dt INT NOT NULL, flight INT NOT NULL, status INT, lat REAL, lon REAL, alt REAL, speed REAL, hdop REAL, vdop REAL, pdop REAL, course REAL, deviation REAL);")
            c.execute("CREATE TABLE IF NOT EXISTS pidConfig (dt INT NOT NULL, flight INT NOT NULL, which INT, p REAL NOT NULL, i REAL NOT NULL, d REAL NOT NULL, max REAL NOT NULL, min REAL NOT NULL, settings INT NOT NULL);")
            sensors = ",".join([x+" REAL" for x in ["ax","ay","az","gx","gy","gz","cx","cy","cz","baro"]])
            c.execute("CREATE TABLE IF NOT EXISTS sensors (dt INT NOT NULL, flight INT NOT NULL, "+sensors+");")
            c.execute("CREATE TABLE IF NOT EXISTS batteryLog (dt INT NOT NULL, flight INT NOT NULL, "+",".join(["cell"+str(x) for x in range(4)])+");")
            conn.commit()
            c.close()
            AT.c = True
        except:
            print "NO SQLLITE DB"
            AT.c = None
            raise
        
        gotit = False
        while gotit is False:
            try:
                AT.s.bind( ("",AT.udpTo[1]) )
                gotit = True
            except:
                sleep(0.1)
        while AT.thread_go:
            data = AT.s.recv(1024)
            AT.packet(data)
